package it.unicam.cs.cosy.bioshape.core.collisionresponse;

import it.unicam.cs.cosy.bioshape.core.components.EntityPair;
import it.unicam.cs.cosy.bioshape.core.components.ShapedEntity;
import it.unicam.cs.cosy.bioshape.core.components.WallEntity;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import vclip.DistanceReport;

/**
 * Implements the "Impulse-based collision response". The approach followed is
 * based on the idea that, at the moment of impact, a great force is applied to
 * the bodies. Such force, called "impulse" sets the bodies apart and realises
 * the response. The bodies are considered rigid (no compression/dilatation) and
 * neither friction, nor constraints (over compound bodies) are taken in
 * account.
 * 
 * @author Federico Buti
 */
public class ImpulseCollisionResponder implements CollisionResponder {

    // private static final double EPSILON_ZERO = 1E-14;

    private void reflect(ShapedEntity e, Vector3d tvel, Vector3d nrm) {
        if (nrm.x != 0) {
            tvel.x = -tvel.x;
        }
        if (nrm.y != 0) {
            tvel.y = -tvel.y;
        }
        if (nrm.z != 0) {
            tvel.z = -tvel.z;
        }
        e.setInstantVelocity(tvel);
    }

    @Override
    public void resolveElastically(EntityPair pair) {
        ShapedEntity e1 = (ShapedEntity) pair.getE1();
        ShapedEntity e2 = (ShapedEntity) pair.getE2();
        // Set Collided
        e1.setCollided(true);
        e2.setCollided(true);
        // Ingredients
        // Velocities
        Vector3d v1 = e1.getInstantVelocity();
        Vector3d v2 = e2.getInstantVelocity();
        // Normal of contact, in world coordinates, away from body 1
        pair.getDistanceReport().transformFirstPoints(e1.getTransform());
        Vector3d n = new Vector3d(
                pair.getDistanceReport().getClosestPair().nrml);
        // Angular velocities
        Vector3d omega1 = e1.getInstantAngularVelocity();
        Vector3d omega2 = e2.getInstantAngularVelocity();

        System.out.println("\n************************");
        System.out.println("COLLISION RESPONSE BETWEEN " + e1.getName()
                + " and " + e2.getName());
        System.out.println("**********************\n");
        // Masses
        double mass1 = e1.getMass();
        double mass2 = e2.getMass();
        // // Inertia Tensors - first version, probably incorrect
        // Matrix3d I1 = new Matrix3d();
        // e1.inertiaTensor(I1);
        // I1.mul(mass1);
        // Matrix3d I2 = new Matrix3d();
        // e2.inertiaTensor(I2);
        // I2.mul(mass2);
        // // Inverses of Inertia Tensors
        // Matrix3d I1Inv = new Matrix3d(I1);
        // I1Inv.invert();
        // Matrix3d I2Inv = new Matrix3d(I2);
        // I2Inv.invert();
        // // Inverses of Rotation Matrices = their transposes
        // Matrix3d R1Inv = new Matrix3d();
        // e1.getTransform().getRotationScale(R1Inv);
        // R1Inv.transpose();
        // Matrix3d R2Inv = new Matrix3d();
        // e2.getTransform().getRotationScale(R2Inv);
        // R2Inv.transpose();
        // Centres of mass of e1 and e2 in world coordinates
        Vector3d p1 = e1.getCenter();
        Vector3d p2 = e2.getCenter();
        // Points of contact in world coordinates WRT centers of mass p_i
        Vector3d r1 = new Vector3d(
                pair.getDistanceReport().getClosestPair().pnt1);
        r1.sub(p1);
        pair.getDistanceReport().transformSecondPoints(e2.getTransform());
        Vector3d r2 = new Vector3d(
                pair.getDistanceReport().getClosestPair().pnt2);
        r2.sub(p2);
        // vp_i
        Vector3d vp1 = new Vector3d();
        vp1.cross(omega1, r1);
        vp1.add(v1);
        Vector3d vp2 = new Vector3d();
        vp2.cross(omega2, r2);
        vp2.add(v2);
        // vr
        Vector3d vr = new Vector3d();
        vr.sub(vp2, vp1);

        // // q_i // from pdf, now using Wikipedia suggestion
        // Vector3d q1 = new Vector3d();
        // q1.cross(r1, n);
        // Matrix3d tmp = new Matrix3d();
        // tmp.mul(I1Inv, R1Inv);
        // tmp.transform(q1);
        // Vector3d q2 = new Vector3d();
        // q2.cross(r2, n);
        // tmp.mul(I2Inv, R2Inv);
        // tmp.transform(q2);
        // // lambda
        // double lambdaNumerator = v1.dot(n) - v2.dot(n);
        // Vector3d tmpVector = new Vector3d(omega1);
        // I1.transform(tmpVector);
        // lambdaNumerator += q1.dot(tmpVector);
        // tmpVector.set(omega2);
        // I2.transform(tmpVector);
        // lambdaNumerator -= q2.dot(tmpVector);
        // double lambdaDenominator = ((1 / mass1) + (1 / mass2)) * n.dot(n);
        // tmpVector.set(q1);
        // I1.transform(tmpVector);
        // lambdaDenominator += q1.dot(tmpVector);
        // tmpVector.set(q2);
        // I2.transform(tmpVector);
        // lambdaDenominator += q2.dot(tmpVector);
        // double lambda = 2 * lambdaNumerator / lambdaDenominator;
        // // New Angular Velocities
        // Vector3d omega1New = new Vector3d();
        // omega1New.scaleAdd(-1 * lambda, q1, omega1);
        // Vector3d omega2New = new Vector3d();
        // omega2New.scaleAdd(lambda, q2, omega2);
        // e1.setInstantAngularVelocity(omega1New);
        // e2.setInstantAngularVelocity(omega2New);
        // // New Velocities
        // double v1NewLength = v1.length() * (m1-m2)
        // Vector3d v1New = new Vector3d();
        // v1New.scaleAdd(-1 * lambda / mass1, n, v1);
        // e1.setInstantVelocity(v1New);
        // Vector3d v2New = new Vector3d();
        // v2New.scaleAdd(lambda / mass2, n, v2);
        // e2.setInstantVelocity(v2New);

        // Orientations and their inverses (transposes)
        Matrix3d R1 = new Matrix3d();
        e1.getTransform().getRotationScale(R1);
        Matrix3d R1Inv = new Matrix3d(R1);
        R1Inv.transpose();
        Matrix3d R2 = new Matrix3d();
        e2.getTransform().getRotationScale(R2);
        Matrix3d R2Inv = new Matrix3d(R2);
        R2Inv.transpose();
        /*
         * Inverse of Inertia Tensors in World Coordinates = R_i * Inertia
         * Tensor Body_i ^-1 * R_i^-1
         */
        Matrix3d I1Body = new Matrix3d();
        e1.inertiaTensor(I1Body);
        I1Body.invert();
        Matrix3d tmp = new Matrix3d();
        tmp.mul(I1Body, R1Inv);
        Matrix3d I1WorldInv = new Matrix3d();
        I1WorldInv.mul(R1, tmp);
        Matrix3d I2Body = new Matrix3d();
        e2.inertiaTensor(I2Body);
        I2Body.invert();
        tmp.mul(I2Body, R2Inv);
        Matrix3d I2WorldInv = new Matrix3d();
        I2WorldInv.mul(R2, tmp);
        /*
         * See http://en.wikipedia.org/wiki/Collision_response - Impulse-based
         * Reaction Model
         */
        // Coefficient of restitution
        double e = 1;
        // Reaction Impulse magnitude jr
        double jrNumerator = -(1 + e) * vr.dot(n);
        double jrDenominator = 0;
        // Treat case of Wall Entities
        if (e1.getClass() == WallEntity.class)
            jrDenominator = 1 / mass2;
        else if (e2.getClass() == WallEntity.class)
            jrDenominator = 1 / mass1;
        else
            jrDenominator = (1 / mass1) + (1 / mass2);
        Vector3d I1WorldInvTransformR1CrossN = new Vector3d();
        I1WorldInvTransformR1CrossN.cross(r1, n);
        I1WorldInv.transform(I1WorldInvTransformR1CrossN);
        Vector3d tmp1 = new Vector3d();
        tmp1.cross(I1WorldInvTransformR1CrossN, r1);
        Vector3d I2WorldInvTransformR2CrossN = new Vector3d();
        I2WorldInvTransformR2CrossN.cross(r2, n);
        I2WorldInv.transform(I2WorldInvTransformR2CrossN);
        Vector3d tmp2 = new Vector3d();
        tmp2.cross(I2WorldInvTransformR2CrossN, r2);
        tmp1.add(tmp2);
        jrDenominator += tmp1.dot(n);
        double jr = jrNumerator / jrDenominator;
        System.out.println("Impulse jr = " + jr);
        // Reaction Impulse vector jrVec
        Vector3d jrVec = new Vector3d(n.x, n.y, n.z);
        jrVec.scale(jr);
        // New Instant Velocities
        /*
         * Two cases must be distinguished: a collision with a wall or a
         * collision between two normal shapes
         */
        if (e1.getClass() == WallEntity.class
                || e2.getClass() == WallEntity.class) {
            System.out.println("Collision Response with a Wall... Reflecting!");
            if (e1.getClass() != WallEntity.class) {
                reflect(e1, v1, n);
            }
            if (e2.getClass() != WallEntity.class) {
                reflect(e2, v2, n);

            }
        } else {
            /*
             * Otherwise proceed normally
             */
            Vector3d tmpVec = new Vector3d(n.x, n.y, n.z);
            Vector3d v1New = new Vector3d(v1);
            tmpVec.scale(jr / mass1);
            v1New.sub(tmpVec);
            Vector3d v2New = new Vector3d(v2);
            tmpVec = new Vector3d(n.x, n.y, n.z);
            tmpVec.scale(jr / mass2);
            v2New.add(tmpVec);
            e1.setInstantVelocity(v1New);
            e2.setInstantVelocity(v2New);
        }
        // New Angular Velocities
        I1WorldInvTransformR1CrossN.scale(jr);
        Vector3d omega1New = new Vector3d(omega1);
        omega1New.sub(I1WorldInvTransformR1CrossN);
        I2WorldInvTransformR2CrossN.scale(jr);
        Vector3d omega2New = new Vector3d(omega2);
        omega2New.add(I2WorldInvTransformR2CrossN);
        e1.setInstantAngularVelocity(omega1New);
        e2.setInstantAngularVelocity(omega2New);
        return;
        // commonCode(e1, e2, pair.getDistanceReport(), e);
    }

    // @Override
    // public void resolveElastically(EntityPair ep) {
    // ShapedEntity se1 = (ShapedEntity) ep.getE1();
    // ShapedEntity se2 = (ShapedEntity) ep.getE2();
    // if (se1.getClass() != WallEntity.class
    // && se2.getClass() != WallEntity.class) {
    // System.out.println("\n************************");
    // System.out.println("COLLISION RESPONSE BETWEEN " + se1.getName()
    // + " and " + se2.getName());
    // System.out.println("**********************\n");
    // }
    // // COF
    // double e = 1;
    // //
    // // backup
    // Vector3d vel1 = se1.getInstantVelocity();
    // Vector3d vel2 = se2.getInstantVelocity();
    // // System.out.println("--->" + vel1);
    // // System.out.println("--->" + vel2);
    // //
    // commonCode(se1, se2, ep.getDistanceReport(), e);
    // //
    // // if (se1.getClass() == WallEntity.class
    // // || se2.getClass() == WallEntity.class) {
    // // reflecting
    // if (se1.getClass() != WallEntity.class)
    // reflect(se1, vel1, ep.getDistanceReport().getClosestPair().nrml);
    // // else
    // if (se2.getClass() != WallEntity.class)
    // reflect(se2, vel2, ep.getDistanceReport().getClosestPair().nrml);
    // // }
    // // System.out.println("---------");
    // // System.out.println("--->" + vel1);
    // // System.out.println("--->" + vel2);
    // }

    /**
     * Execute the common response code for elastic and inelastic collisions.
     * 
     * @param se1
     *            First {@code ShapedEntity} involved in the collision.
     * @param se2
     *            First {@code ShapedEntity} involved in the collision.
     * @param drep
     *            Record for the collision information.
     * @param e
     *            The COR (Coefficient Of Restitution) which can be 0 (inelastic
     *            collision) or 1 (elastic collision).
     */
    private void commonCode(ShapedEntity se1, ShapedEntity se2,
            DistanceReport drep, double e) {
        se1.setCollided(true);
        se2.setCollided(true);
        //
        double ma = se1.getMass();
        double mb = se2.getMass();
        // INERTIA TENSOR1 = rot I_1_body rotT (Baraff)
        Matrix3d m1 = se1.getInertiaTensor();
        Matrix3d rot = new Matrix3d();
        se1.getTransform().get(rot);
        Matrix3d rotT = new Matrix3d(rot);
        rotT.transpose();
        m1.mul(rot, m1);
        m1.mul(m1, rotT);
        //
        // INERTIA TENSOR2 = rot I_2_body rotT (Baraff)
        Matrix3d m2 = se2.getInertiaTensor();
        Matrix3d rot2 = new Matrix3d();
        se2.getTransform().get(rot2);
        Matrix3d rotT2 = new Matrix3d(rot2);
        rotT2.transpose();
        m2.mul(rot2, m2);
        m2.mul(m2, rotT2);
        //
        // Ra --> contact point offset w.r.t. center
        Vector3d center1 = se1.getCenter();
        Vector3d Ra = new Vector3d(drep.getClosestPair().pnt1);
        Ra.sub(center1); // final offset for A
        // Rb --> contact point offset w.r.t. center
        Vector3d center2 = se2.getCenter();
        Vector3d Rb = new Vector3d(drep.getClosestPair().pnt2);
        Rb.sub(center2); // final offset for B
        //
        // normal to the collision ---> returned by V-Clip
        Vector3d nrm = new Vector3d();
        nrm.normalize(drep.getClosestPair().nrml);
        //
        // calculate denominator of impulse j
        // I_a-1 (Ra cross nrm) cross Ra
        if (ma != Double.POSITIVE_INFINITY) // avoid inversion if infinite
            m1.invert();
        Vector3d angularVelChangea = new Vector3d();
        angularVelChangea.cross(Ra, nrm);
        m1.transform(angularVelChangea);
        Vector3d vaLinDueToR = new Vector3d();
        vaLinDueToR.cross(angularVelChangea, Ra);
        // I_b-1 (Rb cross nrm) cross Rb
        if (mb != Double.POSITIVE_INFINITY) // avoid inversion if infinite
            m2.invert();
        Vector3d angularVelChangeb = new Vector3d();
        angularVelChangeb.cross(Rb, nrm);
        m2.transform(angularVelChangeb);
        Vector3d vbLinDueToR = new Vector3d();
        vbLinDueToR.cross(angularVelChangeb, Rb);
        //
        // final denominator following standard formulation (the dot product is
        // split since it is distributive)
        double invMa = Double.isInfinite(ma) ? 0 : 1 / ma;
        double invMb = Double.isInfinite(mb) ? 0 : 1 / mb;
        double scalar = invMa + invMb + vaLinDueToR.dot(nrm)
                + vbLinDueToR.dot(nrm);
        //
        // calculating numerator following standard formulation ---> -(1 + e)
        // ((v_b + omega_b cross Rb) - (v_a + omega_a cross Ra)) dot nrm
        Vector3d vellDiff = new Vector3d();
        Vector3d omegaCrossRa = new Vector3d();
        omegaCrossRa.cross(se1.getInstantAngularVelocity(), Ra);
        Vector3d omegaCrossRb = new Vector3d();
        omegaCrossRb.cross(se2.getInstantAngularVelocity(), Rb);
        // to calculate sum with the cross product of angular velocity with Ra
        Vector3d velCopya = se1.getInstantVelocity();
        velCopya.add(omegaCrossRa);
        Vector3d velCopyb = se2.getInstantVelocity();
        velCopyb.add(omegaCrossRb);
        vellDiff.sub(velCopyb, velCopya);
        //
        // here the final j coefficient
        double Jmod = -(1 + e) * (vellDiff.dot(nrm)) / scalar;
        //
        // calculate impulse for A and B
        Vector3d J = new Vector3d(nrm); // set the normal
        J.scale(Jmod); // product between normal and j
        //
        Vector3d ja = new Vector3d(J);
        Vector3d jb = new Vector3d(J);
        // scale by the masses
        ja.scale(invMa);
        jb.scale(invMb);
        // print test..
        //
        //
        // update translations
        Vector3d tra1 = se1.getInstantVelocity();
        Vector3d tra2 = se2.getInstantVelocity();
        tra1.sub(ja);
        tra2.add(jb);
        //
        // update rotations
        Vector3d rotv1 = se1.getInstantAngularVelocity();
        Vector3d rotv2 = se2.getInstantAngularVelocity();
        angularVelChangea.scale(Jmod);
        angularVelChangeb.scale(Jmod);
        rotv1.sub(angularVelChangea);
        rotv2.add(angularVelChangeb);
        se1.setInstantAngularVelocity(rotv1);
        se2.setInstantAngularVelocity(rotv2);
    }

    @Override
    public void resolveInelastically(EntityPair ep) {
        ShapedEntity se1 = (ShapedEntity) ep.getE1();
        ShapedEntity se2 = (ShapedEntity) ep.getE2();
        // COF
        double e = 0;
        // backup
        Vector3d vel1 = se1.getInstantVelocity();
        Vector3d vel2 = se2.getInstantVelocity();
        // TODO backup for rotations too??
        commonCode(se1, se2, ep.getDistanceReport(), e);
        //
        // TODO something for inelastic Luca: O_O
    }
}
